\chapter{实验分析}
\label{cha:analize}
\section{实验环境}
\label{sec:history}
\begin{itemize}
    \item ubuntu18
    \item c++ 11 
    \item cmake 3 
    \item g2o eigen opencv 等c++库
\end{itemize}

\section{仿真实验}

由于我们只关注BA最终的优化效果，所以我们选取了华盛顿大学\cite{RN16}的开源课程提供的dataset。该数据集包含了相机位姿的初始值以及landmark的初始值，我们可以专注于优化
而不用将精力分散到如何处理图像等问题上。

该数据及第一行记录了相机数、路标数量、总的观测数量，从第二行起，分别记录了第i个相机所观测的第j个路标所看到的像素坐标。这就构成了BA问题。同时该课程提供了一些相关的函数供我们调用，
包括转化为点云的api等。

同时，我选用了g2o这个开源库作为工具，g2o是一个图优化的库，由于我们求解的问题具有很好的矩阵稀疏性以及图的特性，我们可以利用图优化的方法对问题进行加速。
我们只需要实现BA的solver,带入到框架中就可以高效的解决我们的问题。

为了使用g2o,我们需要对图框架的节点以及节点之间的关系函数进行重载(override)

\subsection{节点定义}
\begin{lstlisting}[title=Myfile, frame=shadowbox]
    class VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        VertexCameraBAL() {}
    
        virtual bool read ( std::istream& /*is*/ )
        {
            return false;
        }
    
        virtual bool write ( std::ostream& /*os*/ ) const
        {
            return false;
        }
    
        virtual void setToOriginImpl() {}
    
        virtual void oplusImpl ( const double* update )
        {
            Eigen::VectorXd::ConstMapType v ( update, VertexCameraBAL::Dimension );
            _estimate += v;
        }
    
    };    
    class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexPointBAL() {}

    virtual bool read ( std::istream& /*is*/ )
    {
        return false;
    }

    virtual bool write ( std::ostream& /*os*/ ) const
    {
        return false;
    }

    virtual void setToOriginImpl() {}

    virtual void oplusImpl ( const double* update )
    {
        Eigen::Vector3d::ConstMapType v ( update );
        _estimate += v;
    }
};
\end{lstlisting}

\subsection{关联函数定义}
\begin{lstlisting}
class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d,VertexCameraBAL, VertexPointBAL>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeObservationBAL() {}

    virtual bool read ( std::istream& /*is*/ )
    {
        return false;
    }

    virtual bool write ( std::ostream& /*os*/ ) const
    {
        return false;
    }

    virtual void computeError() override   // The virtual function comes from the Edge base class. Must define if you use edge.
    {
        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );

        ( *this ) ( cam->estimate().data(), point->estimate().data(), _error.data() );

    }

    template<typename T>
    bool operator() ( const T* camera, const T* point, T* residuals ) const
    {
        T predictions[2];
        CamProjectionWithDistortion ( camera, point, predictions );
        residuals[0] = predictions[0] - T ( measurement() ( 0 ) );
        residuals[1] = predictions[1] - T ( measurement() ( 1 ) );

        return true;
    }


    virtual void linearizeOplus() override
    {
        // use numeric Jacobians
        // BaseBinaryEdge<2, Vector2d, VertexCameraBAL, VertexPointBAL>::linearizeOplus();
        // return;
        
        // using autodiff from ceres. Otherwise, the system will use g2o numerical diff for Jacobians

        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
        typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;

        Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
        Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
        double *parameters[] = { const_cast<double*> ( cam->estimate().data() ), const_cast<double*> ( point->estimate().data() ) };
        double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
        double value[Dimension];
        bool diffState = BalAutoDiff::Differentiate ( *this, parameters, Dimension, value, jacobians );

        // copy over the Jacobians (convert row-major -> column-major)
        if ( diffState )
        {
            _jacobianOplusXi = dError_dCamera;
            _jacobianOplusXj = dError_dPoint;
        }
        else
        {
            assert ( 0 && "Error while differentiating" );
            _jacobianOplusXi.setZero();
            _jacobianOplusXj.setZero();
        }
    }
};
\end{lstlisting}

以上部分实现了BA的框架，需要注意的是，对于Jacobian矩阵的求解我们使用了ceres库进行加速。同时利用了Hessian矩阵的稀疏性，我们进行了Marginalize，即求该矩阵的舒尔补。

接下来我们需要读取dataset中的数据转化为我们现有的BA问题。

\subsection{函数主体}


\begin{lstlisting}
    #include <Eigen/StdVector>
#include <Eigen/Core>

#include <iostream>
#include <stdint.h>

#include <unordered_set>
#include <memory>
#include <vector>
#include <stdlib.h> 

#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/optimization_algorithm_dogleg.h"

#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/types/sba/types_six_dof_expmap.h"

#include "g2o/solvers/structure_only/structure_only_solver.h"

#include "common/BundleParams.h"
#include "common/BALProblem.h"
#include "g2o_bal_class.h"


using namespace Eigen;
using namespace std;

typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9,3> > BalBlockSolver;

// set up the vertexs and edges for the bundle adjustment. 
void BuildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer, const BundleParams& params)
{
    const int num_points = bal_problem->num_points();
    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    const int point_block_size = bal_problem->point_block_size();

    // Set camera vertex with initial value in the dataset.
    const double* raw_cameras = bal_problem->cameras();
    for(int i = 0; i < num_cameras; ++i)
    {
        ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i,camera_block_size);
        VertexCameraBAL* pCamera = new VertexCameraBAL();
        pCamera->setEstimate(temVecCamera);   // initial value for the camera i..
        pCamera->setId(i);                    // set id for each camera vertex 
  
        // remeber to add vertex into optimizer..
        optimizer->addVertex(pCamera);
        
    }

    // Set point vertex with initial value in the dataset. 
    const double* raw_points = bal_problem->points();
    // const int point_block_size = bal_problem->point_block_size();
    for(int j = 0; j < num_points; ++j)
    {
        ConstVectorRef temVecPoint(raw_points + point_block_size * j, point_block_size);
        VertexPointBAL* pPoint = new VertexPointBAL();
        pPoint->setEstimate(temVecPoint);   // initial value for the point i..
        pPoint->setId(j + num_cameras);     // each vertex should have an unique id, no matter it is a camera vertex, or a point vertex 

        // remeber to add vertex into optimizer..
        pPoint->setMarginalized(true);
        optimizer->addVertex(pPoint);
    }

    // Set edges for graph..
    const int  num_observations = bal_problem->num_observations();
    const double* observations = bal_problem->observations();   // pointer for the first observation..

    for(int i = 0; i < num_observations; ++i)
    {
        EdgeObservationBAL* bal_edge = new EdgeObservationBAL();

        const int camera_id = bal_problem->camera_index()[i]; // get id for the camera; 
        const int point_id = bal_problem->point_index()[i] + num_cameras; // get id for the point 
        
        if(params.robustify)
        {
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
            rk->setDelta(1.0);
            bal_edge->setRobustKernel(rk);
        }
        // set the vertex by the ids for an edge observation
        bal_edge->setVertex(0,dynamic_cast<VertexCameraBAL*>(optimizer->vertex(camera_id)));
        bal_edge->setVertex(1,dynamic_cast<VertexPointBAL*>(optimizer->vertex(point_id)));
        bal_edge->setInformation(Eigen::Matrix2d::Identity());
        bal_edge->setMeasurement(Eigen::Vector2d(observations[2*i+0],observations[2*i + 1]));

       optimizer->addEdge(bal_edge) ;
    }

}

void WriteToBALProblem(BALProblem* bal_problem, g2o::SparseOptimizer* optimizer)
{
    const int num_points = bal_problem->num_points();
    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    const int point_block_size = bal_problem->point_block_size();

    double* raw_cameras = bal_problem->mutable_cameras();
    for(int i = 0; i < num_cameras; ++i)
    {
        VertexCameraBAL* pCamera = dynamic_cast<VertexCameraBAL*>(optimizer->vertex(i));
        Eigen::VectorXd NewCameraVec = pCamera->estimate();
        memcpy(raw_cameras + i * camera_block_size, NewCameraVec.data(), sizeof(double) * camera_block_size);
    }

    double* raw_points = bal_problem->mutable_points();
    for(int j = 0; j < num_points; ++j)
    {
        VertexPointBAL* pPoint = dynamic_cast<VertexPointBAL*>(optimizer->vertex(j + num_cameras));
        Eigen::Vector3d NewPointVec = pPoint->estimate();
        memcpy(raw_points + j * point_block_size, NewPointVec.data(), sizeof(double) * point_block_size);
    }
}

//this function is  unused yet..
void SetMinimizerOptions(std::shared_ptr<BalBlockSolver>& solver_ptr, const BundleParams& params, g2o::SparseOptimizer* optimizer)
{
    //std::cout<<"Set Minimizer  .."<< std::endl;
    g2o::OptimizationAlgorithmWithHessian* solver;
    if(params.trust_region_strategy == "levenberg_marquardt"){
        solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr.get());
    }
    else if(params.trust_region_strategy == "dogleg"){
        solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr.get());
    }
    else 
    {
        std::cout << "Please check your trust_region_strategy parameter again.."<< std::endl;
        exit(EXIT_FAILURE);
    }

    optimizer->setAlgorithm(solver);
    //std::cout<<"Set Minimizer  .."<< std::endl;
}

//this function is  unused yet..
void SetLinearSolver(std::shared_ptr<BalBlockSolver>& solver_ptr, const BundleParams& params)
{
    //std::cout<<"Set Linear Solver .."<< std::endl;
    g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver = 0;
    
    if(params.linear_solver == "dense_schur" ){
        linearSolver = new g2o::LinearSolverDense<BalBlockSolver::PoseMatrixType>();
    }
    else if(params.linear_solver == "sparse_schur"){
        linearSolver = new g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>();
        dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>* >(linearSolver)->setBlockOrdering(true);  // AMD ordering , only needed for sparse cholesky solver
    }
    

    solver_ptr = std::make_shared<BalBlockSolver>(linearSolver);
    std::cout <<  "Set Complete.."<< std::endl;
}

void SetSolverOptionsFromFlags(BALProblem* bal_problem, const BundleParams& params, g2o::SparseOptimizer* optimizer)
{   
    BalBlockSolver* solver_ptr;
    
    
    g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver = 0;
    
    if(params.linear_solver == "dense_schur" ){
        linearSolver = new g2o::LinearSolverDense<BalBlockSolver::PoseMatrixType>();
    }
    else if(params.linear_solver == "sparse_schur"){
        linearSolver = new g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>();
        dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>* >(linearSolver)->setBlockOrdering(true);  // AMD ordering , only needed for sparse cholesky solver
    }
    

    solver_ptr = new BalBlockSolver(linearSolver);
    //SetLinearSolver(solver_ptr, params);

    //SetMinimizerOptions(solver_ptr, params, optimizer);
    g2o::OptimizationAlgorithmWithHessian* solver;
    if(params.trust_region_strategy == "levenberg_marquardt"){
        solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    }
    else if(params.trust_region_strategy == "dogleg"){
        solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr);
    }
    else 
    {
        std::cout << "Please check your trust_region_strategy parameter again.."<< std::endl;
        exit(EXIT_FAILURE);
    }

    optimizer->setAlgorithm(solver);
}


void SolveProblem(const char* filename, const BundleParams& params)
{
    BALProblem bal_problem(filename);

    // show some information here ...
    std::cout << "bal problem file loaded..." << std::endl;
    std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points. " << std::endl;
    std::cout << "Forming " << bal_problem.num_observations() << " observatoins. " << std::endl;

    // store the initial 3D cloud points and camera pose..
    if(!params.initial_ply.empty()){
        bal_problem.WriteToPLYFile(params.initial_ply);
    }

    std::cout << "beginning problem..." << std::endl;
    
    // add some noise for the intial value
    srand(params.random_seed);
    bal_problem.Normalize();
    bal_problem.Perturb(params.rotation_sigma, params.translation_sigma,
                        params.point_sigma);

    std::cout << "Normalization complete..." << std::endl;


    g2o::SparseOptimizer optimizer;
    SetSolverOptionsFromFlags(&bal_problem, params, &optimizer);
    BuildProblem(&bal_problem, &optimizer, params);

    
    std::cout << "begin optimizaiton .."<< std::endl;
    // perform the optimizaiton 
    optimizer.initializeOptimization();
    optimizer.setVerbose(true);
    optimizer.optimize(params.num_iterations);

    std::cout << "optimization complete.. "<< std::endl;
    // write the optimized data into BALProblem class
    WriteToBALProblem(&bal_problem, &optimizer);

    // write the result into a .ply file.
    if(!params.final_ply.empty()){
        bal_problem.WriteToPLYFile(params.final_ply);
    }
   
}

int main(int argc, char** argv)
{
    
    BundleParams params(argc,argv);  // set the parameters here.

    if(params.input.empty()){
        std::cout << "Usage: bundle_adjuster -input <path for dataset>";
        return 1;
    }

    SolveProblem(params.input.c_str(), params);
  
    return 0;
}
\end{lstlisting}


\subsection{实验结果}
实验结果如下：
\begin{lstlisting}
Header: 16 22106 83718bal problem file loaded...
bal problem have 16 cameras and 22106 points. 
Forming 83718 observatoins. 
beginning problem...
Normalization complete...
begin optimizaiton ..
iteration= 0     chi2= 1593976.900329    time= 0.084214  cumTime= 0.084214       edges= 83718    schur= 1        lambda= 9370.114177     levenbergIter= 1
iteration= 1     chi2= 1503859.189085    time= 0.0697507         cumTime= 0.153965       edges= 83718    schur= 1        lambda= 3123.371392     levenbergIter= 1
iteration= 2     chi2= 1340948.148116    time= 0.0693824         cumTime= 0.223347       edges= 83718    schur= 1        lambda= 1041.123797     levenbergIter= 1
iteration= 3     chi2= 1104714.717556    time= 0.0694619         cumTime= 0.292809       edges= 83718    schur= 1        lambda= 347.041266      levenbergIter= 1
iteration= 4     chi2= 879105.141579     time= 0.0691852         cumTime= 0.361994       edges= 83718    schur= 1        lambda= 115.680422      levenbergIter= 1
iteration= 5     chi2= 723703.972964     time= 0.0700193         cumTime= 0.432013       edges= 83718    schur= 1        lambda= 38.560141       levenbergIter= 1
iteration= 6     chi2= 615709.929828     time= 0.0691596         cumTime= 0.501173       edges= 83718    schur= 1        lambda= 12.853380       levenbergIter= 1
iteration= 7     chi2= 503775.361750     time= 0.0699948         cumTime= 0.571168       edges= 83718    schur= 1        lambda= 4.284460        levenbergIter= 1
iteration= 8     chi2= 381980.060879     time= 0.0697287         cumTime= 0.640897       edges= 83718    schur= 1        lambda= 1.428153        levenbergIter= 1
iteration= 9     chi2= 267385.644652     time= 0.0700045         cumTime= 0.710901       edges= 83718    schur= 1        lambda= 0.476051        levenbergIter= 1
iteration= 10    chi2= 170314.069365     time= 0.0703536         cumTime= 0.781255       edges= 83718    schur= 1        lambda= 0.158684        levenbergIter= 1
iteration= 11    chi2= 108750.143596     time= 0.0700822         cumTime= 0.851337       edges= 83718    schur= 1        lambda= 0.052895        levenbergIter= 1
iteration= 12    chi2= 79228.387608      time= 0.0699536         cumTime= 0.921291       edges= 83718    schur= 1        lambda= 0.035263        levenbergIter= 1
iteration= 13    chi2= 62903.760510      time= 0.0712769         cumTime= 0.992567       edges= 83718    schur= 1        lambda= 0.023509        levenbergIter= 1
iteration= 14    chi2= 51637.899996      time= 0.0703801         cumTime= 1.06295        edges= 83718    schur= 1        lambda= 0.015672        levenbergIter= 1
iteration= 15    chi2= 42491.525780      time= 0.0700455         cumTime= 1.13299        edges= 83718    schur= 1        lambda= 0.010448        levenbergIter= 1
iteration= 16    chi2= 37296.044380      time= 0.071258  cumTime= 1.20425        edges= 83718    schur= 1        lambda= 0.005843        levenbergIter= 1
iteration= 17    chi2= 36145.312948      time= 0.0706447         cumTime= 1.2749         edges= 83718    schur= 1        lambda= 0.001948        levenbergIter= 1
iteration= 18    chi2= 36069.306555      time= 0.0694338         cumTime= 1.34433        edges= 83718    schur= 1        lambda= 0.000649        levenbergIter= 1
iteration= 19    chi2= 36067.831214      time= 0.0699527         cumTime= 1.41428        edges= 83718    schur= 1        lambda= 0.000216        levenbergIter= 1
optimization complete.. 
\end{lstlisting}

我们调用api生成了初始的点云与最终的点云。

\begin{figure}[htbp]
    \centering
    \includegraphics[height=5cm]{initial.png}
    \caption{初始状态点云}
    \label{fig:initial}
  \end{figure}


  \begin{figure}[htbp]
    \centering
    \includegraphics[height=5cm]{final.PNG}
    \caption{最终状态的点云}
    \label{fig:final}
  \end{figure}

从图中我们可以看出BA的优化效果非常显著，从程序的log也可以看出我们的$\lambda$下降的非常迅速。
\section{实验总结}

从实验中我们可以发现BA的优化效果非常明显，并且在图优化、稀疏矩阵加速的加持下，实时性也得到了保障，具有非常好的效果。